drivers/console: Reimplement MUTLI_CONSOLE_API framework in C
authorJulius Werner <[email protected]>
Wed, 28 Nov 2018 06:10:56 +0000 (22:10 -0800)
committerJulius Werner <[email protected]>
Fri, 7 Dec 2018 00:18:10 +0000 (16:18 -0800)
Now that we have switched to using the stack in MULTI_CONSOLE_API
framework functions and have factored all code involved in crash
reporting out into a separate file, there's really no reason to keep the
main framework code in assembly anymore. This patch rewrites it in C
which allows us to have a single implementation across aarch32/64 and
should be much easier to maintain going forward.

Change-Id: I6c85a01e89a79e8b233f3f8bee812f0dbd026221
Signed-off-by: Julius Werner <[email protected]>
Makefile
drivers/console/aarch32/multi_console.S [deleted file]
drivers/console/aarch64/multi_console.S [deleted file]
drivers/console/multi_console.c [new file with mode: 0644]

index 63f283f3e25f2e13c10b7d39b9eb388ef2d5d01e..eed7c7a8559e8fc76d4b1e8d73c4c9a124fdd8da 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -214,7 +214,7 @@ include lib/libc/libc.mk
 BL_COMMON_SOURCES      +=      common/bl_common.c                      \
                                common/tf_log.c                         \
                                common/${ARCH}/debug.S                  \
-                               drivers/console/${ARCH}/multi_console.S \
+                               drivers/console/multi_console.c         \
                                lib/${ARCH}/cache_helpers.S             \
                                lib/${ARCH}/misc_helpers.S              \
                                plat/common/plat_bl_common.c            \
diff --git a/drivers/console/aarch32/multi_console.S b/drivers/console/aarch32/multi_console.S
deleted file mode 100644 (file)
index 713dc3c..0000000
+++ /dev/null
@@ -1,322 +0,0 @@
-/*
- * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#if MULTI_CONSOLE_API
-
-#include <asm_macros.S>
-#include <assert_macros.S>
-#include <console.h>
-
-       .globl  console_register
-       .globl  console_unregister
-       .globl  console_is_registered
-       .globl  console_set_scope
-       .globl  console_switch_state
-       .globl  console_putc
-       .globl  console_getc
-       .globl  console_flush
-
-       /*
-        *  The console list pointer is in the data section and not in
-        *  .bss even though it is zero-init. In particular, this allows
-        *  the console functions to start using this variable before
-        *  the runtime memory is initialized for images which do not
-        *  need to copy the .data section from ROM to RAM.
-        */
-.section .data.console_list ; .align 2
-       console_list: .word 0x0
-.section .data.console_state ; .align 0
-       console_state: .byte CONSOLE_FLAG_BOOT
-
-       /* -----------------------------------------------
-        * int console_register(console_t *console)
-        * Function to insert a new console structure into
-        * the console list. Should usually be called by
-        * console_<driver>_register implementations. The
-        * data structure passed will be taken over by the
-        * console framework and *MUST* be allocated in
-        * persistent memory (e.g. the data section).
-        * In : r0 - address of console_t structure
-        * Out: r0 - Always 1 (for easier tail calling)
-        * Clobber list: r0, r1
-        * -----------------------------------------------
-        */
-func console_register
-       push    {r6,  lr}
-#if ENABLE_ASSERTIONS
-       /* Assert that r0 isn't a NULL pointer */
-       cmp     r0, #0
-       ASM_ASSERT(ne)
-       /* Assert that the struct isn't in the stack */
-       ldr     r1, =__STACKS_START__
-       cmp     r0, r1
-       blo     not_on_stack
-       ldr     r1, =__STACKS_END__
-       cmp     r0, r1
-       ASM_ASSERT(hs)
-not_on_stack:
-       /* Assert that this struct isn't in the list */
-       mov     r1, r0 /* Preserve r0 and lr */
-       bl      console_is_registered
-       cmp     r0, #0
-       ASM_ASSERT(eq)
-       mov     r0, r1
-#endif /* ENABLE_ASSERTIONS */
-       ldr     r6, =console_list
-       ldr     r1, [r6]        /* R1 = first struct in list */
-       str     r0, [r6]        /* list head = new console */
-       str     r1, [r0, #CONSOLE_T_NEXT]       /* new console next ptr = R1 */
-       mov     r0, #1
-       pop     {r6, pc}
-endfunc console_register
-
-       /* -----------------------------------------------
-        * int console_unregister(console_t *console)
-        * Function to find a specific console in the list
-        * of currently active consoles and remove it.
-        * In: r0 - address of console_t struct to remove
-        * Out: r0 - removed address, or NULL if not found
-        * Clobber list: r0, r1
-        * -----------------------------------------------
-        */
-func console_unregister
-#if ENABLE_ASSERTIONS
-       /* Assert that r0 isn't a NULL pointer */
-       cmp     r0, #0
-       ASM_ASSERT(ne)
-#endif /* ENABLE_ASSERTIONS */
-       push    {r6}
-       ldr     r6, =console_list               /* R6 = ptr to first struct */
-       ldr     r1, [r6]                        /* R1 = first struct */
-
-unregister_loop:
-       cmp     r1, #0
-       beq     unregister_not_found
-       cmp     r0, r1
-       beq     unregister_found
-       ldr     r6, [r6]                        /* R6 = next ptr of struct */
-       ldr     r1, [r6]                        /* R1 = next struct */
-       b       unregister_loop
-
-unregister_found:
-       ldr     r1, [r1]                        /* R1 = next struct */
-       str     r1, [r6]                        /* prev->next = cur->next */
-       pop     {r6}
-       bx      lr
-
-unregister_not_found:
-       mov     r0, #0                          /* return NULL if not found */
-       pop     {r6}
-       bx      lr
-endfunc console_unregister
-
-       /* -----------------------------------------------
-        * int console_is_registered(console_t *console)
-        * Function to detect if a specific console is
-        * registered or not.
-        * In: r0 - address of console_t struct to remove
-        * Out: r0 - 1 if it is registered, 0 if not.
-        * Clobber list: r0
-        * -----------------------------------------------
-        */
-func console_is_registered
-#if ENABLE_ASSERTIONS
-       /* Assert that r0 isn't a NULL pointer */
-       cmp     r0, #0
-       ASM_ASSERT(ne)
-#endif /* ENABLE_ASSERTIONS */
-       push    {r6}
-       ldr     r6, =console_list
-       ldr     r6, [r6]        /* R6 = first console struct */
-check_registered_loop:
-       cmp     r6, #0                  /* Check if end of list */
-       beq     console_not_registered
-       cmp     r0, r6          /* Check if the pointers are different */
-       beq     console_registered
-       ldr     r6, [r6, #CONSOLE_T_NEXT]       /* Get pointer to next struct */
-       b       check_registered_loop
-console_not_registered:
-       mov     r0, #0
-       pop     {r6}
-       bx      lr
-console_registered:
-       mov     r0, #1
-       pop     {r6}
-       bx      lr
-endfunc console_is_registered
-
-       /* -----------------------------------------------
-        * void console_switch_state(unsigned int new_state)
-        * Function to switch the current console state.
-        * The console state determines which of the
-        * registered consoles are actually used at a time.
-        * In : r0 - global console state to move to
-        * Clobber list: r0, r1
-        * -----------------------------------------------
-        */
-func console_switch_state
-       ldr     r1, =console_state
-       strb    r0, [r1]
-       bx      lr
-endfunc console_switch_state
-
-       /* -----------------------------------------------
-        * void console_set_scope(console_t *console,
-        *                       unsigned int scope)
-        * Function to update the states that a given console
-        * may be active in.
-        * In : r0 - pointer to console_t struct
-        *    : r1 - new active state mask
-        * Clobber list: r0, r1, r2
-        * -----------------------------------------------
-        */
-func console_set_scope
-#if ENABLE_ASSERTIONS
-       ands    r2, r1, #~CONSOLE_FLAG_SCOPE_MASK
-       ASM_ASSERT(eq)
-#endif /* ENABLE_ASSERTIONS */
-       ldr     r2, [r0, #CONSOLE_T_FLAGS]
-       and     r2, r2, #~CONSOLE_FLAG_SCOPE_MASK
-       orr     r2, r2, r1
-       str     r2, [r0, #CONSOLE_T_FLAGS]
-       bx      lr
-endfunc console_set_scope
-
-       /* ---------------------------------------------
-        * int console_putc(int c)
-        * Function to output a character. Calls all
-        * active console's putc() handlers in succession.
-        * In : r0 - character to be printed
-        * Out: r0 - printed character on success, or < 0
-                    if at least one console had an error
-        * Clobber list : r0, r1, r2
-        * ---------------------------------------------
-        */
-func console_putc
-       push    {r4-r6, lr}
-       mov     r5, #ERROR_NO_VALID_CONSOLE     /* R5 = current return value */
-       mov     r4, r0                          /* R4 = character to print */
-       ldr     r6, =console_list
-       ldr     r6, [r6]        /* R6 = first console struct */
-
-putc_loop:
-       cmp     r6, #0
-       beq     putc_done
-       ldr     r1, =console_state
-       ldrb    r1, [r1]
-       ldr     r2, [r6, #CONSOLE_T_FLAGS]
-       tst     r1, r2
-       beq     putc_continue
-       ldr     r2, [r6, #CONSOLE_T_PUTC]
-       cmp     r2, #0
-       beq     putc_continue
-       mov     r0, r4
-       mov     r1, r6
-       blx     r2
-       cmp     r5, #ERROR_NO_VALID_CONSOLE     /* update R5 if it's NOVALID */
-       cmpne   r0, #0                          /* else update it if R0 < 0 */
-       movlt   r5, r0
-putc_continue:
-       ldr     r6, [r6]                        /* R6 = next struct */
-       b       putc_loop
-
-putc_done:
-       mov     r0, r5
-       pop     {r4-r6, pc}
-endfunc console_putc
-
-       /* ---------------------------------------------
-        * int console_getc(void)
-        * Function to get a character from any console.
-        * Keeps looping through all consoles' getc()
-        * handlers until one of them returns a
-        * character, then stops iterating and returns
-        * that character to the caller. Will stop looping
-        * if all active consoles report real errors
-        * (other than just not having a char available).
-        * Out : r0 - read character, or < 0 on error
-        * Clobber list : r0, r1
-        * ---------------------------------------------
-        */
-func console_getc
-       push    {r5-r6, lr}
-getc_try_again:
-       mov     r5, #ERROR_NO_VALID_CONSOLE     /* R5 = current return value */
-       ldr     r6, =console_list
-       ldr     r6, [r6]                        /* R6 = first console struct */
-       cmp     r6, #0
-       bne     getc_loop
-       mov     r0, r5                          /* If no consoles registered */
-       pop     {r5-r6, pc}                     /* return immediately. */
-
-getc_loop:
-       ldr     r0, =console_state
-       ldrb    r0, [r0]
-       ldr     r1, [r6, #CONSOLE_T_FLAGS]
-       tst     r0, r1
-       beq     getc_continue
-       ldr     r1, [r6, #CONSOLE_T_GETC]
-       cmp     r1, #0
-       beq     getc_continue
-       mov     r0, r6
-       blx     r1
-       cmp     r0, #0                          /* if R0 >= 0: return */
-       bge     getc_found
-       cmp     r5, #ERROR_NO_PENDING_CHAR      /* may update R5 (NOCHAR has */
-       movne   r5, r0                          /* precedence vs real errors) */
-getc_continue:
-       ldr     r6, [r6]                        /* R6 = next struct */
-       cmp     r6, #0
-       bne     getc_loop
-       cmp     r5, #ERROR_NO_PENDING_CHAR      /* Keep scanning if at least */
-       beq     getc_try_again                  /* one console returns NOCHAR */
-       mov     r0, r5
-
-getc_found:
-       pop     {r5-r6, pc}
-endfunc console_getc
-
-       /* ---------------------------------------------
-        * int console_flush(void)
-        * Function to force a write of all buffered
-        * data that hasn't been output. Calls all
-        * console's flush() handlers in succession.
-        * Out: r0 - 0 on success, < 0 if at least one error
-        * Clobber list : r0, r1, r2
-        * ---------------------------------------------
-        */
-func console_flush
-       push    {r5-r6, lr}
-       mov     r5, #ERROR_NO_VALID_CONSOLE     /* R5 = current return value */
-       ldr     r6, =console_list
-       ldr     r6, [r6]                        /* R6 = first console struct */
-
-flush_loop:
-       cmp     r6, #0
-       beq     flush_done
-       ldr     r1, =console_state
-       ldrb    r1, [r1]
-       ldr     r2, [r6, #CONSOLE_T_FLAGS]
-       tst     r1, r2
-       beq     flush_continue
-       ldr     r1, [r6, #CONSOLE_T_FLUSH]
-       cmp     r1, #0
-       beq     flush_continue
-       mov     r0, r6
-       blx     r1
-       cmp     r5, #ERROR_NO_VALID_CONSOLE     /* update R5 if it's NOVALID */
-       cmpne   r0, #0                          /* else update it if R0 < 0 */
-       movlt   r5, r0
-flush_continue:
-       ldr     r6, [r6]                        /* R6 = next struct */
-       b       flush_loop
-
-flush_done:
-       mov     r0, r5
-       pop     {r5-r6, pc}
-endfunc console_flush
-
-#endif /* MULTI_CONSOLE_API */
diff --git a/drivers/console/aarch64/multi_console.S b/drivers/console/aarch64/multi_console.S
deleted file mode 100644 (file)
index 40d500d..0000000
+++ /dev/null
@@ -1,329 +0,0 @@
-/*
- * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#if MULTI_CONSOLE_API
-
-#include <asm_macros.S>
-#include <assert_macros.S>
-#include <console.h>
-
-       .globl  console_register
-       .globl  console_unregister
-       .globl  console_is_registered
-       .globl  console_set_scope
-       .globl  console_switch_state
-       .globl  console_putc
-       .globl  console_getc
-       .globl  console_flush
-       .globl  console_list
-
-       /*
-        *  The console list pointer is in the data section and not in
-        *  .bss even though it is zero-init. In particular, this allows
-        *  the console functions to start using this variable before
-        *  the runtime memory is initialized for images which do not
-        *  need to copy the .data section from ROM to RAM.
-        */
-.section .data.console_list ; .align 3
-       console_list: .quad 0x0
-.section .data.console_state ; .align 0
-       console_state: .byte CONSOLE_FLAG_BOOT
-
-       /* -----------------------------------------------
-        * int console_register(console_t *console)
-        * Function to insert a new console structure into
-        * the console list. Should usually be called by
-        * console_<driver>_register implementations. The
-        * data structure passed will be taken over by the
-        * console framework and *MUST* be allocated in
-        * persistent memory (e.g. the data section).
-        * In : x0 - address of console_t structure
-        * Out: x0 - Always 1 (for easier tail calling)
-        * Clobber list: x0, x1
-        * -----------------------------------------------
-        */
-func console_register
-       stp     x21, x30, [sp, #-16]!
-#if ENABLE_ASSERTIONS
-       /* Assert that x0 isn't a NULL pointer */
-       cmp     x0, #0
-       ASM_ASSERT(ne)
-       /* Assert that the struct isn't in the stack */
-       adrp    x1, __STACKS_START__
-       add     x1, x1, :lo12:__STACKS_START__
-       cmp     x0, x1
-       b.lo    not_on_stack
-       adrp    x1, __STACKS_END__
-       add     x1, x1, :lo12:__STACKS_END__
-       cmp     x0, x1
-       ASM_ASSERT(hs)
-not_on_stack:
-       /* Assert that this struct isn't in the list */
-       mov     x1, x0 /* Preserve x0 and x30 */
-       bl      console_is_registered
-       cmp     x0, #0
-       ASM_ASSERT(eq)
-       mov     x0, x1
-#endif /* ENABLE_ASSERTIONS */
-       adrp    x21, console_list
-       ldr     x1, [x21, :lo12:console_list]   /* X1 = first struct in list */
-       str     x0, [x21, :lo12:console_list]   /* list head = new console */
-       str     x1, [x0, #CONSOLE_T_NEXT]       /* new console next ptr = X1 */
-       mov     x0, #1
-       ldp     x21, x30, [sp], #16
-       ret
-endfunc console_register
-
-       /* -----------------------------------------------
-        * int console_unregister(console_t *console)
-        * Function to find a specific console in the list
-        * of currently active consoles and remove it.
-        * In: x0 - address of console_t struct to remove
-        * Out: x0 - removed address, or NULL if not found
-        * Clobber list: x0, x1
-        * -----------------------------------------------
-        */
-func console_unregister
-#if ENABLE_ASSERTIONS
-       /* Assert that x0 isn't a NULL pointer */
-       cmp     x0, #0
-       ASM_ASSERT(ne)
-#endif /* ENABLE_ASSERTIONS */
-       stp     x21, xzr, [sp, #-16]!
-       adrp    x21, console_list
-       add     x21, x21, :lo12:console_list    /* X21 = ptr to first struct */
-       ldr     x1, [x21]                       /* X1 = first struct */
-
-unregister_loop:
-       cbz     x1, unregister_not_found
-       cmp     x0, x1
-       b.eq    unregister_found
-       ldr     x21, [x21]                      /* X21 = next ptr of struct */
-       ldr     x1, [x21]                       /* X1 = next struct */
-       b       unregister_loop
-
-unregister_found:
-       ldr     x1, [x1]                        /* X1 = next struct */
-       str     x1, [x21]                       /* prev->next = cur->next */
-       ldp     x21, xzr, [sp], #16
-       ret
-
-unregister_not_found:
-       mov     x0, #0                          /* return NULL if not found */
-       ldp     x21, xzr, [sp], #16
-       ret
-endfunc console_unregister
-
-       /* -----------------------------------------------
-        * int console_is_registered(console_t *console)
-        * Function to detect if a specific console is
-        * registered or not.
-        * In: x0 - address of console_t struct to remove
-        * Out: x0 - 1 if it is registered, 0 if not.
-        * Clobber list: x0
-        * -----------------------------------------------
-        */
-func console_is_registered
-#if ENABLE_ASSERTIONS
-       /* Assert that x0 isn't a NULL pointer */
-       cmp     x0, #0
-       ASM_ASSERT(ne)
-#endif /* ENABLE_ASSERTIONS */
-       stp     x21, xzr, [sp, #-16]!
-       adrp    x21, console_list
-       ldr     x21, [x21, :lo12:console_list]  /* X21 = first console struct */
-check_registered_loop:
-       cbz     x21, console_not_registered /* Check if end of list */
-       cmp     x0, x21         /* Check if the pointers are different */
-       b.eq    console_registered
-       ldr     x21, [x21, #CONSOLE_T_NEXT]     /* Get pointer to next struct */
-       b       check_registered_loop
-console_not_registered:
-       mov     x0, #0
-       ldp     x21, xzr, [sp], #16
-       ret
-console_registered:
-       mov     x0, #1
-       ldp     x21, xzr, [sp], #16
-       ret
-endfunc console_is_registered
-
-       /* -----------------------------------------------
-        * void console_switch_state(unsigned int new_state)
-        * Function to switch the current console state.
-        * The console state determines which of the
-        * registered consoles are actually used at a time.
-        * In : w0 - global console state to move to
-        * Clobber list: x0, x1
-        * -----------------------------------------------
-        */
-func console_switch_state
-       adrp    x1, console_state
-       strb    w0, [x1, :lo12:console_state]
-       ret
-endfunc console_switch_state
-
-       /* -----------------------------------------------
-        * void console_set_scope(console_t *console,
-        *                       unsigned int scope)
-        * Function to update the states that a given console
-        * may be active in.
-        * In : x0 - pointer to console_t struct
-        *    : w1 - new active state mask
-        * Clobber list: x0, x1, x2
-        * -----------------------------------------------
-        */
-func console_set_scope
-#if ENABLE_ASSERTIONS
-       tst     w1, #~CONSOLE_FLAG_SCOPE_MASK
-       ASM_ASSERT(eq)
-#endif /* ENABLE_ASSERTIONS */
-       ldr     w2, [x0, #CONSOLE_T_FLAGS]
-       and     w2, w2, #~CONSOLE_FLAG_SCOPE_MASK
-       orr     w2, w2, w1
-       str     w2, [x0, #CONSOLE_T_FLAGS]
-       ret
-endfunc console_set_scope
-
-       /* ---------------------------------------------
-        * int console_putc(int c)
-        * Function to output a character. Calls all
-        * active console's putc() handlers in succession.
-        * In : x0 - character to be printed
-        * Out: x0 - printed character on success, or < 0
-                    if at least one console had an error
-        * Clobber list : x0, x1, x2
-        * ---------------------------------------------
-        */
-func console_putc
-       stp     x21, x30, [sp, #-16]!
-       stp     x19, x20, [sp, #-16]!
-       mov     w20, #ERROR_NO_VALID_CONSOLE    /* W20 = current return value */
-       mov     w19, w0                         /* W19 = character to print */
-       adrp    x21, console_list
-       ldr     x21, [x21, :lo12:console_list]  /* X21 = first console struct */
-
-putc_loop:
-       cbz     x21, putc_done
-       adrp    x1, console_state
-       ldrb    w1, [x1, :lo12:console_state]
-       ldr     w2, [x21, #CONSOLE_T_FLAGS]
-       tst     w1, w2
-       b.eq    putc_continue
-       ldr     x2, [x21, #CONSOLE_T_PUTC]
-       cbz     x2, putc_continue
-       mov     w0, w19
-       mov     x1, x21
-       blr     x2
-       cmp     w20, #ERROR_NO_VALID_CONSOLE    /* update W20 if it's NOVALID */
-       ccmp    w0, #0, #0x8, ne                /* else update it if W0 < 0 */
-       csel    w20, w0, w20, lt
-putc_continue:
-       ldr     x21, [x21]                      /* X21 = next struct */
-       b       putc_loop
-
-putc_done:
-       mov     w0, w20
-       ldp     x19, x20, [sp], #16
-       ldp     x21, x30, [sp], #16
-       ret
-endfunc console_putc
-
-       /* ---------------------------------------------
-        * int console_getc(void)
-        * Function to get a character from any console.
-        * Keeps looping through all consoles' getc()
-        * handlers until one of them returns a
-        * character, then stops iterating and returns
-        * that character to the caller. Will stop looping
-        * if all active consoles report real errors
-        * (other than just not having a char available).
-        * Out : x0 - read character, or < 0 on error
-        * Clobber list : x0, x1
-        * ---------------------------------------------
-        */
-func console_getc
-       stp     x30, xzr, [sp, #-16]!
-       stp     x20, x21, [sp, #-16]!
-getc_try_again:
-       mov     w20, #ERROR_NO_VALID_CONSOLE    /* W20 = current return value */
-       adrp    x21, console_list
-       ldr     x21, [x21, :lo12:console_list]  /* X21 = first console struct */
-       cbnz    x21, getc_loop
-       mov     w0, w20                         /* If no consoles registered */
-       ldp     x20, x21, [sp], #16
-       ldp     x30, xzr, [sp], #16
-       ret                                     /* return immediately. */
-
-getc_loop:
-       adrp    x0, console_state
-       ldrb    w0, [x0, :lo12:console_state]
-       ldr     w1, [x21, #CONSOLE_T_FLAGS]
-       tst     w0, w1
-       b.eq    getc_continue
-       ldr     x1, [x21, #CONSOLE_T_GETC]
-       cbz     x1, getc_continue
-       mov     x0, x21
-       blr     x1
-       cmp     w0, #0                          /* if X0 >= 0: return */
-       b.ge    getc_found
-       cmp     w20, #ERROR_NO_PENDING_CHAR     /* may update W20 (NOCHAR has */
-       csel    w20, w20, w0, eq                /* precedence vs real errors) */
-getc_continue:
-       ldr     x21, [x21]                      /* X21 = next struct */
-       cbnz    x21, getc_loop
-       cmp     w20, #ERROR_NO_PENDING_CHAR     /* Keep scanning if at least */
-       b.eq    getc_try_again                  /* one console returns NOCHAR */
-       mov     w0, w20
-
-getc_found:
-       ldp     x20, x21, [sp], #16
-       ldp     x30, xzr, [sp], #16
-       ret
-endfunc console_getc
-
-       /* ---------------------------------------------
-        * int console_flush(void)
-        * Function to force a write of all buffered
-        * data that hasn't been output. Calls all
-        * console's flush() handlers in succession.
-        * Out: x0 - 0 on success, < 0 if at least one error
-        * Clobber list : x0, x1, x2
-        * ---------------------------------------------
-        */
-func console_flush
-       stp     x30, xzr, [sp, #-16]!
-       stp     x20, x21, [sp, #-16]!
-       mov     w20, #ERROR_NO_VALID_CONSOLE    /* W20 = current return value */
-       adrp    x21, console_list
-       ldr     x21, [x21, :lo12:console_list]  /* X21 = first console struct */
-
-flush_loop:
-       cbz     x21, flush_done
-       adrp    x1, console_state
-       ldrb    w1, [x1, :lo12:console_state]
-       ldr     w2, [x21, #CONSOLE_T_FLAGS]
-       tst     w1, w2
-       b.eq    flush_continue
-       ldr     x1, [x21, #CONSOLE_T_FLUSH]
-       cbz     x1, flush_continue
-       mov     x0, x21
-       blr     x1
-       cmp     w20, #ERROR_NO_VALID_CONSOLE    /* update W20 if it's NOVALID */
-       ccmp    w0, #0, #0x8, ne                /* else update it if W0 < 0 */
-       csel    w20, w0, w20, lt
-flush_continue:
-       ldr     x21, [x21]                      /* X21 = next struct */
-       b       flush_loop
-
-flush_done:
-       mov     w0, w20
-       ldp     x20, x21, [sp], #16
-       ldp     x30, xzr, [sp], #16
-       ret
-endfunc console_flush
-
-#endif /* MULTI_CONSOLE_API */
diff --git a/drivers/console/multi_console.c b/drivers/console/multi_console.c
new file mode 100644 (file)
index 0000000..c678de0
--- /dev/null
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if MULTI_CONSOLE_API
+
+#include <assert.h>
+#include <drivers/console.h>
+
+console_t *console_list;
+uint8_t console_state = CONSOLE_FLAG_BOOT;
+
+int console_register(console_t *console)
+{
+       IMPORT_SYM(console_t *, __STACKS_START__, stacks_start)
+       IMPORT_SYM(console_t *, __STACKS_END__, stacks_end)
+
+       /* Assert that the struct is not on the stack (common mistake). */
+       assert((console < stacks_start) || (console >= stacks_end));
+       /* Assert that we won't make a circle in the list. */
+       assert(!console_is_registered(console));
+
+       console->next = console_list;
+       console_list = console;
+
+       /* Return 1 for convenient tail-calling from console_xxx_register(). */
+       return 1;
+}
+
+console_t *console_unregister(console_t *to_be_deleted)
+{
+       console_t **ptr;
+
+       assert(to_be_deleted != NULL);
+
+       for (ptr = &console_list; *ptr != NULL; ptr = &(*ptr)->next)
+               if (*ptr == to_be_deleted) {
+                       *ptr = (*ptr)->next;
+                       return to_be_deleted;
+               }
+
+       return NULL;
+}
+
+int console_is_registered(console_t *to_find)
+{
+       console_t *console;
+
+       assert(to_find != NULL);
+
+       for (console = console_list; console != NULL; console = console->next)
+               if (console == to_find)
+                       return 1;
+
+       return 0;
+}
+
+void console_switch_state(unsigned int new_state)
+{
+       console_state = new_state;
+}
+
+void console_set_scope(console_t *console, unsigned int scope)
+{
+       assert(console != NULL);
+
+       console->flags = (console->flags & ~CONSOLE_FLAG_SCOPE_MASK) | scope;
+}
+
+int console_putc(int c)
+{
+       int err = ERROR_NO_VALID_CONSOLE;
+       console_t *console;
+
+       for (console = console_list; console != NULL; console = console->next)
+               if (console->flags & console_state) {
+                       int ret = console->putc(c, console);
+                       if ((err == ERROR_NO_VALID_CONSOLE) || (ret < err))
+                               err = ret;
+               }
+
+       return err;
+}
+
+int console_getc(void)
+{
+       int err = ERROR_NO_VALID_CONSOLE;
+       console_t *console;
+
+       do {    /* Keep polling while at least one console works correctly. */
+               for (console = console_list; console != NULL;
+                    console = console->next)
+                       if (console->flags & console_state) {
+                               int ret = console->getc(console);
+                               if (ret >= 0)
+                                       return ret;
+                               if (err != ERROR_NO_PENDING_CHAR)
+                                       err = ret;
+                       }
+       } while (err == ERROR_NO_PENDING_CHAR);
+
+       return err;
+}
+
+int console_flush(void)
+{
+       int err = ERROR_NO_VALID_CONSOLE;
+       console_t *console;
+
+       for (console = console_list; console != NULL; console = console->next)
+               if (console->flags & console_state) {
+                       int ret = console->flush(console);
+                       if ((err == ERROR_NO_VALID_CONSOLE) || (ret < err))
+                               err = ret;
+               }
+
+       return err;
+}
+
+#endif /* MULTI_CONSOLE_API */